From afcfdda960da9d9ad4c4d21c7dd0cc7791cf36c7 Mon Sep 17 00:00:00 2001
From: Madalin Bucur <madalin.bucur@freescale.com>
Date: Wed, 4 May 2016 19:24:53 +0300
Subject: [PATCH 29/70] fmd: SGMII PCS needs to be reprogrammed after sleep

Signed-off-by: Madalin Bucur <madalin.bucur@freescale.com>
---
 drivers/net/ethernet/freescale/sdk_dpaa/dpaa_eth.c |    6 ++
 .../freescale/sdk_fman/Peripherals/FM/MAC/dtsec.c  |    1 +
 .../freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.c |   12 ++++
 .../freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.h |    1 +
 .../freescale/sdk_fman/Peripherals/FM/MAC/memac.c  |   58 +++++++++++---------
 .../freescale/sdk_fman/Peripherals/FM/MAC/tgec.c   |    1 +
 .../sdk_fman/inc/Peripherals/fm_mac_ext.h          |   13 +++++
 .../sdk_fman/src/inc/wrapper/lnxwrp_fsl_fman.h     |    2 +
 .../freescale/sdk_fman/src/wrapper/lnxwrp_fm.c     |   14 +++++
 9 files changed, 83 insertions(+), 25 deletions(-)

--- a/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_eth.c
+++ b/drivers/net/ethernet/freescale/sdk_dpaa/dpaa_eth.c
@@ -175,6 +175,12 @@ static int dpaa_resume(struct device *de
 		priv = netdev_priv(net_dev);
 		mac_dev = priv->mac_dev;
 
+		err = fm_mac_resume(mac_dev->get_mac_handle(mac_dev));
+		if (err) {
+			netdev_err(net_dev, "fm_mac_resume = %d\n", err);
+			goto resume_failed;
+		}
+
 		err = fm_port_resume(mac_dev->port_dev[TX]);
 		if (err) {
 			netdev_err(net_dev, "fm_port_resume(TX) = %d\n", err);
--- a/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/dtsec.c
+++ b/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/dtsec.c
@@ -1371,6 +1371,7 @@ static void InitFmMacControllerDriver(t_
 
     p_FmMacControllerDriver->f_FM_MAC_Enable                    = DtsecEnable;
     p_FmMacControllerDriver->f_FM_MAC_Disable                   = DtsecDisable;
+    p_FmMacControllerDriver->f_FM_MAC_Resume                    = NULL;
 
     p_FmMacControllerDriver->f_FM_MAC_SetException              = DtsecSetException;
 
--- a/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.c
+++ b/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.c
@@ -299,6 +299,18 @@ t_Error FM_MAC_Disable (t_Handle h_FmMac
     RETURN_ERROR(MINOR, E_NOT_SUPPORTED, NO_MSG);
 }
 
+t_Error FM_MAC_Resume (t_Handle h_FmMac)
+{
+    t_FmMacControllerDriver *p_FmMacControllerDriver = (t_FmMacControllerDriver *)h_FmMac;
+
+    SANITY_CHECK_RETURN_ERROR(p_FmMacControllerDriver, E_INVALID_HANDLE);
+
+    if (p_FmMacControllerDriver->f_FM_MAC_Resume)
+        return p_FmMacControllerDriver->f_FM_MAC_Resume(h_FmMac);
+
+    return E_OK;
+}
+
 /* ......................................................................... */
 
 t_Error FM_MAC_Enable1588TimeStamp (t_Handle h_FmMac)
--- a/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.h
+++ b/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/fm_mac.h
@@ -91,6 +91,7 @@ typedef struct {
 
     t_Error (*f_FM_MAC_Enable)  (t_Handle h_FmMac,  e_CommMode mode);
     t_Error (*f_FM_MAC_Disable) (t_Handle h_FmMac, e_CommMode mode);
+    t_Error (*f_FM_MAC_Resume)  (t_Handle h_FmMac);
     t_Error (*f_FM_MAC_Enable1588TimeStamp) (t_Handle h_FmMac);
     t_Error (*f_FM_MAC_Disable1588TimeStamp) (t_Handle h_FmMac);
     t_Error (*f_FM_MAC_Reset)   (t_Handle h_FmMac, bool wait);
--- a/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/memac.c
+++ b/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/memac.c
@@ -812,6 +812,37 @@ static uint16_t MemacGetMaxFrameLength(t
     return fman_memac_get_max_frame_len(p_Memac->p_MemMap);
 }
 
+static t_Error MemacInitInternalPhy(t_Handle h_Memac)
+{
+    t_Memac *p_Memac = (t_Memac *)h_Memac;
+    uint8_t i, phyAddr;
+
+    if (ENET_INTERFACE_FROM_MODE(p_Memac->enetMode) == e_ENET_IF_SGMII)
+    {
+        /* Configure internal SGMII PHY */
+        if (p_Memac->enetMode & ENET_IF_SGMII_BASEX)
+            SetupSgmiiInternalPhyBaseX(p_Memac, PHY_MDIO_ADDR);
+        else
+            SetupSgmiiInternalPhy(p_Memac, PHY_MDIO_ADDR);
+    }
+    else if (ENET_INTERFACE_FROM_MODE(p_Memac->enetMode) == e_ENET_IF_QSGMII)
+    {
+        /* Configure 4 internal SGMII PHYs */
+        for (i = 0; i < 4; i++)
+        {
+            /* QSGMII PHY address occupies 3 upper bits of 5-bit
+               phyAddress; the lower 2 bits are used to extend
+               register address space and access each one of 4
+               ports inside QSGMII. */
+            phyAddr = (uint8_t)((PHY_MDIO_ADDR << 2) | i);
+            if (p_Memac->enetMode & ENET_IF_SGMII_BASEX)
+                SetupSgmiiInternalPhyBaseX(p_Memac, phyAddr);
+            else
+                SetupSgmiiInternalPhy(p_Memac, phyAddr);
+        }
+    }
+    return E_OK;
+}
 
 /*****************************************************************************/
 /*                      mEMAC Init & Free API                                   */
@@ -825,7 +856,6 @@ static t_Error MemacInit(t_Handle h_Mema
     struct memac_cfg        *p_MemacDriverParam;
     enum enet_interface     enet_interface;
     enum enet_speed         enet_speed;
-    uint8_t                 i, phyAddr;
     t_EnetAddr              ethAddr;
     e_FmMacType             portType;
     t_Error                 err;
@@ -887,30 +917,7 @@ static t_Error MemacInit(t_Handle h_Mema
     }
 #endif /* FM_RX_FIFO_CORRUPT_ERRATA_10GMAC_A006320 */
 
-    if (ENET_INTERFACE_FROM_MODE(p_Memac->enetMode) == e_ENET_IF_SGMII)
-    {
-        /* Configure internal SGMII PHY */
-        if (p_Memac->enetMode & ENET_IF_SGMII_BASEX)
-            SetupSgmiiInternalPhyBaseX(p_Memac, PHY_MDIO_ADDR);
-        else
-            SetupSgmiiInternalPhy(p_Memac, PHY_MDIO_ADDR);
-    }
-    else if (ENET_INTERFACE_FROM_MODE(p_Memac->enetMode) == e_ENET_IF_QSGMII)
-    {
-        /* Configure 4 internal SGMII PHYs */
-        for (i = 0; i < 4; i++)
-        {
-            /* QSGMII PHY address occupies 3 upper bits of 5-bit
-               phyAddress; the lower 2 bits are used to extend
-               register address space and access each one of 4
-               ports inside QSGMII. */
-            phyAddr = (uint8_t)((PHY_MDIO_ADDR << 2) | i);
-            if (p_Memac->enetMode & ENET_IF_SGMII_BASEX)
-                SetupSgmiiInternalPhyBaseX(p_Memac, phyAddr);
-            else
-                SetupSgmiiInternalPhy(p_Memac, phyAddr);
-        }
-    }
+    MemacInitInternalPhy(h_Memac);
 
     /* Max Frame Length */
     err = FmSetMacMaxFrame(p_Memac->fmMacControllerDriver.h_Fm,
@@ -1008,6 +1015,7 @@ static void InitFmMacControllerDriver(t_
 
     p_FmMacControllerDriver->f_FM_MAC_Enable                    = MemacEnable;
     p_FmMacControllerDriver->f_FM_MAC_Disable                   = MemacDisable;
+    p_FmMacControllerDriver->f_FM_MAC_Resume                    = MemacInitInternalPhy;
 
     p_FmMacControllerDriver->f_FM_MAC_SetTxAutoPauseFrames      = MemacSetTxAutoPauseFrames;
     p_FmMacControllerDriver->f_FM_MAC_SetTxPauseFrames          = MemacSetTxPauseFrames;
--- a/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/tgec.c
+++ b/drivers/net/ethernet/freescale/sdk_fman/Peripherals/FM/MAC/tgec.c
@@ -897,6 +897,7 @@ static void InitFmMacControllerDriver(t_
 
     p_FmMacControllerDriver->f_FM_MAC_Enable                    = TgecEnable;
     p_FmMacControllerDriver->f_FM_MAC_Disable                   = TgecDisable;
+    p_FmMacControllerDriver->f_FM_MAC_Resume                    = NULL;
 
     p_FmMacControllerDriver->f_FM_MAC_SetTxAutoPauseFrames      = TgecTxMacPause;
     p_FmMacControllerDriver->f_FM_MAC_SetTxPauseFrames          = TgecSetTxPauseFrames;
--- a/drivers/net/ethernet/freescale/sdk_fman/inc/Peripherals/fm_mac_ext.h
+++ b/drivers/net/ethernet/freescale/sdk_fman/inc/Peripherals/fm_mac_ext.h
@@ -467,6 +467,19 @@ t_Error FM_MAC_Enable(t_Handle h_FmMac,
 t_Error FM_MAC_Disable(t_Handle h_FmMac, e_CommMode mode);
 
 /**************************************************************************//**
+ @Function      FM_MAC_Resume
+
+ @Description   Re-init the MAC after suspend
+
+ @Param[in]     h_FmMac    A handle to a FM MAC Module.
+
+ @Return        E_OK on success; Error code otherwise.
+
+ @Cautions      Allowed only following FM_MAC_Init().
+*//***************************************************************************/
+t_Error FM_MAC_Resume(t_Handle h_FmMac);
+
+/**************************************************************************//**
  @Function      FM_MAC_Enable1588TimeStamp
 
  @Description   Enables the TSU operation.
--- a/drivers/net/ethernet/freescale/sdk_fman/src/inc/wrapper/lnxwrp_fsl_fman.h
+++ b/drivers/net/ethernet/freescale/sdk_fman/src/inc/wrapper/lnxwrp_fsl_fman.h
@@ -765,6 +765,8 @@ int fm_mac_enable(struct fm_mac_dev *fm_
 
 int fm_mac_disable(struct fm_mac_dev *fm_mac_dev);
 
+int fm_mac_resume(struct fm_mac_dev *fm_mac_dev);
+
 int fm_mac_set_promiscuous(struct fm_mac_dev *fm_mac_dev,
 		bool enable);
 
--- a/drivers/net/ethernet/freescale/sdk_fman/src/wrapper/lnxwrp_fm.c
+++ b/drivers/net/ethernet/freescale/sdk_fman/src/wrapper/lnxwrp_fm.c
@@ -1733,6 +1733,20 @@ int fm_mac_disable(struct fm_mac_dev *fm
 }
 EXPORT_SYMBOL(fm_mac_disable);
 
+int fm_mac_resume(struct fm_mac_dev *fm_mac_dev)
+{
+        int      _errno;
+        t_Error  err;
+
+        err = FM_MAC_Resume(fm_mac_dev);
+        _errno = -GET_ERROR_TYPE(err);
+        if (unlikely(_errno < 0))
+                pr_err("FM_MAC_Resume() = 0x%08x\n", err);
+
+        return _errno;
+}
+EXPORT_SYMBOL(fm_mac_resume);
+
 int fm_mac_set_promiscuous(struct fm_mac_dev *fm_mac_dev,
 		bool enable)
 {
